#include "ti/driverlib/dl_gpio.h"
#include "ti_msp_dl_config.h"

void Motor_Left_SetSpeed(uint16_t speed)
{
    if(speed>0)
    {
        DL_GPIO_setPins(Motor_PORT, Motor_Left_Front_PIN);
        DL_GPIO_clearPins(Motor_PORT, Motor_Left_Back_PIN);
        DL_TimerG_setCaptureCompareValue(PWM_1_INST, speed, DL_TIMER_CC_0_INDEX);
    }
    else {
        DL_GPIO_setPins(Motor_PORT, Motor_Left_Back_PIN);
        DL_GPIO_clearPins(Motor_PORT, Motor_Left_Front_PIN);
        DL_TimerG_setCaptureCompareValue(PWM_1_INST, -speed, DL_TIMER_CC_0_INDEX);
    }
}

void Motor_Right_SetSpeed(uint16_t speed)
{
    if(speed>0)
    {
        DL_GPIO_setPins(Motor_PORT, Motor_Right_Front_PIN);
        DL_GPIO_clearPins(Motor_PORT, Motor_Right_Back_PIN);
        DL_TimerG_setCaptureCompareValue(PWM_1_INST, speed, DL_TIMER_CC_1_INDEX);
    }
    else {
        DL_GPIO_setPins(Motor_PORT, Motor_Right_Back_PIN);
        DL_GPIO_clearPins(Motor_PORT, Motor_Right_Front_PIN);
        DL_TimerG_setCaptureCompareValue(PWM_1_INST, -speed, DL_TIMER_CC_1_INDEX);
    }
}

void motorPidSetSpeed(float pwm1out,float pwm2out)
{
    Motor_Left_SetSpeed(pwm1out);
    Motor_Right_SetSpeed(pwm2out);
}




double Kp = -19.0;  //-22
double Ki = 0.0;
double Kd = 0.0;
// 计算PID输出
double calculate_pid(double current_angle,double setpoint) {
    static double integral =0,last_error =0;
    double error = setpoint - current_angle;
    integral += error;
    double derivative = error - last_error;
    last_error = error;

    double output = Kp * error + Ki * integral + Kd * derivative;
    return output;
}